/*
 * @file quaternion.cpp 
 * @brief quaternion.
 * @author Sheng Yang, Fei Zhu
 * 
 * This file is part of Physika, a versatile physics simulation library.
 * Copyright (C) 2013- Physika Group.
 *
 * This Source Code Form is subject to the terms of the GNU General Public License v2.0. 
 * If a copy of the GPL was not distributed with this file, you can obtain one at:
 * http://www.gnu.org/licenses/gpl-2.0.html
 *
 */

#include <cmath>
#include "quaternion.h"


namespace PhysLeo{

Quaternion::Quaternion(float x, float y, float z, float w):x_(x),y_(y),z_(z),w_(w)
{
}

Quaternion::Quaternion(const glm::vec3 & unit_axis, float angle_rad)
{
    const auto a = angle_rad * 0.5f;
    const auto s = sin(a);
    w_ = cos(a);
    x_ = unit_axis[0] * s;
    y_ = unit_axis[1] * s;
    z_ = unit_axis[2] * s;
}

glm::vec3 Quaternion::rotate(const glm::vec3& v) const 
{
    const auto vx = 2.0f * v[0];
    const auto vy = 2.0f * v[1];
    const auto vz = 2.0f * v[2];
    const auto w2 = w_ * w_ - 0.5f;
    const auto dot2 = (x_ * vx + y_ * vy + z_ * vz);
    return glm::vec3
        (
        (vx * w2 + (y_ * vz - z_ * vy) * w_ + x_ * dot2), 
        (vy * w2 + (z_ * vx - x_ * vz) * w_ + y_ * dot2), 
        (vz * w2 + (x_ * vy - y_ * vx) * w_ + z_ * dot2)
        );
}

}
